#!/usr/bin/env python3
import time
import threading
import re
from queue import Queue
from typing import Optional, Dict, List, Any, Tuple

import rclpy
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rcl_interfaces.msg import ParameterDescriptor, ParameterType

import serial
from serial.tools import list_ports

from geometry_msgs.msg import Twist
from std_msgs.msg import String, Float32, Bool
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue

from ball_collector_interfaces.srv import SetGripper

class HardwareInterface(Node):
    """
    硬件接口节点，负责与STM32通信
    """
    def __init__(self):
        super().__init__('hardware_interface')
        # 添加串口访问锁
        self.serial_lock = threading.RLock()
        
        # 创建回调组
        self.timer_cb_group = MutuallyExclusiveCallbackGroup()
        self.service_cb_group = ReentrantCallbackGroup()
        
        # 声明参数
        self.declare_parameters(
            namespace='',
            parameters=[
                ('serial_port', '/dev/ttyUSB0',
                 ParameterDescriptor(type=ParameterType.PARAMETER_STRING,
                                     description='Serial port')),
                ('baud_rate', 115200,
                 ParameterDescriptor(type=ParameterType.PARAMETER_INTEGER,
                                     description='Baud rate')),
                ('cmd_timeout', 3.0,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Command timeout (s)')),
                ('auto_discover', True,
                 ParameterDescriptor(type=ParameterType.PARAMETER_BOOL,
                                     description='Auto discover serial port')),
                ('max_linear_speed', 0.5,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Maximum linear speed (m/s)')),
                ('max_angular_speed', 1.0,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Maximum angular speed (rad/s)')),
                ('diagnostic_period', 1.0,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Diagnostic publishing period (s)')),
                ('watchdog_timeout', 0.5,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Watchdog timeout (s)')),
            ]
        )
        
        # 获取参数
        self.serial_port = self.get_parameter('serial_port').value
        self.baud_rate = self.get_parameter('baud_rate').value
        self.cmd_timeout = self.get_parameter('cmd_timeout').value
        self.auto_discover = self.get_parameter('auto_discover').value
        self.max_linear_speed = self.get_parameter('max_linear_speed').value
        self.max_angular_speed = self.get_parameter('max_angular_speed').value
        self.diagnostic_period = self.get_parameter('diagnostic_period').value
        self.watchdog_timeout = self.get_parameter('watchdog_timeout').value
        
        # 初始化串口
        self.ser = None
        self.is_connected = False
        self.last_cmd_time = time.time()
        self.last_received_time = time.time()
        
        # 通信线程和队列
        self.tx_queue = Queue()
        self.rx_queue = Queue()
        self.comm_thread = None
        self.running = True
        
        # 夹爪状态
        self.gripper_state = False
        
        # 初始化串口
        self.init_serial()
        
        # 创建订阅者
        self.cmd_vel_sub = self.create_subscription(
            Twist,
            'cmd_vel',
            self.cmd_vel_callback,
            10
        )
        
        # 创建发布者
        self.status_pub = self.create_publisher(
            String,
            'hardware_status',
            10
        )
        
        self.diagnostic_pub = self.create_publisher(
            DiagnosticArray,
            '/diagnostics',
            10
        )
        
        self.battery_pub = self.create_publisher(
            Float32,
            'battery_voltage',
            10
        )
        
        # 创建服务
        # self.gripper_service = self.create_service(
        #     SetGripper,
        #     'set_gripper',
        #     self.set_gripper_callback,
        #     callback_group=self.service_cb_group
        # )
        self.gripper_service = self.create_service(
            SetGripper,
            '/hardware_interface/set_gripper',  # 使用节点相对名称
            self.set_gripper_callback,
            callback_group=self.service_cb_group
        )
        # 创建定时器
        self.watchdog_timer = self.create_timer(
            self.watchdog_timeout,
            self.watchdog_callback,
            callback_group=self.timer_cb_group
        )
        
        self.diagnostic_timer = self.create_timer(
            self.diagnostic_period,
            self.publish_diagnostics,
            callback_group=self.timer_cb_group
        )
        
        self.get_logger().info("硬件接口节点已初始化")
    
    def init_serial(self):
        """初始化串口通信"""
        if self.auto_discover:
            self.serial_port = self.discover_serial_port()
            if not self.serial_port:
                self.get_logger().error("无法自动发现CH340串口设备")
                return
        try:
            self.ser = serial.Serial(
                port=self.serial_port,
                baudrate=self.baud_rate,
                timeout=1.0
            )
            
            # 清空缓冲区
            self.ser.reset_input_buffer()
            self.ser.reset_output_buffer()
            
            self.is_connected = True
            self.get_logger().info(f"串口已连接: {self.serial_port} @ {self.baud_rate}")
            
            # 确保旧线程已停止
            self.stop_communication_thread()
            
            # 启动通信线程
            self.running = True
            self.comm_thread = threading.Thread(target=self.communication_loop)
            self.comm_thread.daemon = True
            self.comm_thread.start()
            
            # 发送初始化命令
            self.send_command("INIT")
            
        except Exception as e:
            self.get_logger().error(f"串口连接失败: {str(e)}")
            self.is_connected = False
    
    def stop_communication_thread(self):
        """停止通信线程"""
        if hasattr(self, 'comm_thread') and self.comm_thread is not None and self.comm_thread.is_alive():
            self.running = False
            
            self.comm_thread.join(timeout=1.0)
            if self.comm_thread.is_alive():
                self.get_logger().warn("通信线程未能正常停止")
    
    def discover_serial_port(self) -> str:
        """自动发现CH340串口设备"""
        self.get_logger().info("正在自动搜索CH340串口设备...")
        
        ports = list_ports.comports()
        for port in ports:
            # 查找CH340串口适配器
            if 'CH340' in port.description or 'ch340' in port.description.lower():
                self.get_logger().info(f"找到CH340设备: {port.device}")
                return port.device
        
        # 查找USB设备
        for port in ports:
            if 'USB' in port.description and (
                'Serial' in port.description or 
                'serial' in port.description.lower()
            ):
                self.get_logger().info(f"找到USB串口设备: {port.device}")
                return port.device
        
        # 如果没有找到设备，返回默认值
        self.get_logger().warn("未找到CH340设备，使用默认端口")
        return '/dev/ttyUSB0'
    
    def cmd_vel_callback(self, msg):
        """处理速度命令"""
        # 限制速度范围
        linear_x = max(-self.max_linear_speed, min(msg.linear.x, self.max_linear_speed))
        angular_z = max(-self.max_angular_speed, min(msg.angular.z, self.max_angular_speed))
        
        # 将速度转换为STM32命令
        self.send_motor_command(linear_x, angular_z)
        
        # 更新命令时间
        self.last_cmd_time = time.time()
    
    # def set_gripper_callback(self, request, response):
    #     """夹爪控制服务回调"""
    #     cmd = "GRAB" if request.grip else "RELEASE"
    #     force = min(1.0, max(0.0, request.force))  # 限制力度范围
        
    #     # 发送命令
    #     cmd_str = f"{cmd}:{int(force * 100)}"
    #     success = self.send_command(cmd_str)
        
    #     if success:
    #         # 更新状态
    #         self.gripper_state = request.grip
    #         response.success = True
    #         response.message = f"夹爪{'关闭' if request.grip else '打开'}成功"
    #     else:
    #         response.success = False
    #         response.message = "夹爪控制失败"
        
    #     return response

    
    def set_gripper_callback(self, request, response):
        """夹爪控制服务回调 - 线程安全版"""
        cmd = "GRAB" if request.grip else "RELEASE"
        force = min(1.0, max(0.0, request.force))  # 限制力度范围
        
        # 发送命令
        cmd_str = f"{cmd}:{int(force * 100)}"
        success = False
        
        try:
            success = self.send_command(cmd_str)
            
            if success:
                # 更新状态
                self.gripper_state = request.grip
                response.success = True
                response.message = f"夹爪{'关闭' if request.grip else '打开'}成功"
            else:
                response.success = False
                response.message = "夹爪控制失败"
        except Exception as e:
            self.get_logger().error(f"夹爪控制异常: {e}")
            response.success = False
            response.message = f"夹爪控制异常: {str(e)}"
        
        return response
    # def send_motor_command(self, linear_x, angular_z):
    #     """发送电机控制命令"""
    #     # 映射到电机速度
    #     # 假设: 
    #     # - 正的linear_x表示前进
    #     # - 正的angular_z表示逆时针旋转
        
    #     # 简单差速转向计算
    #     left_speed = int((linear_x - angular_z * 0.5) * 100)
    #     right_speed = int((linear_x + angular_z * 0.5) * 100)
        
    #     # 限制在[-100, 100]范围
    #     left_speed = max(-100, min(100, left_speed))
    #     right_speed = max(-100, min(100, right_speed))
        
    #     # 发送命令
    #     cmd = f"MOVE:{left_speed},{right_speed}"
    #     self.send_command(cmd)

    def send_motor_command(self, linear_x, angular_z):
        """发送电机控制命令 - 线程安全版"""
        # 映射到电机速度
        left_speed = int((linear_x - angular_z * 0.5) * 100)
        right_speed = int((linear_x + angular_z * 0.5) * 100)
        
        # 限制在[-100, 100]范围
        left_speed = max(-100, min(100, left_speed))
        right_speed = max(-100, min(100, right_speed))
        
        # 发送命令
        cmd = f"MOVE:{left_speed},{right_speed}"
        return self.send_command(cmd)   
    

    def send_command(self, cmd_str):
        """发送命令到串口队列"""
        if not self.is_connected:
            self.get_logger().warn(f"未连接到串口，无法发送命令: {cmd_str}")
            return False
        
        # 格式化命令
        cmd = f"<{cmd_str}>\n"
        
        # 加入发送队列
        self.tx_queue.put(cmd)
        return True
    
    # def communication_loop(self):
    #     """通信线程主循环"""
    #     self.get_logger().info("通信线程已启动")
        
    #     buffer = ""
        
    #     while self.running:
    #         try:
    #             # 检查发送队列
    #             if not self.tx_queue.empty() and self.is_connected and self.ser and self.ser.is_open:
    #                 cmd = self.tx_queue.get(block=False)
    #                 self.ser.write(cmd.encode())
    #                 self.get_logger().debug(f"发送: {cmd.strip()}")
                
    #             # 检查接收数据
    #             if self.is_connected and self.ser and self.ser.is_open and self.ser.in_waiting:
    #                 try:
    #                     data = self.ser.read(self.ser.in_waiting).decode('utf-8', errors='ignore')
    #                     buffer += data
                        
    #                     # 处理完整的命令
    #                     while True:
    #                         start = buffer.find('<')
    #                         end = buffer.find('>', start)
                            
    #                         if start >= 0 and end >= 0:
    #                             # 提取命令
    #                             cmd = buffer[start+1:end]
    #                             buffer = buffer[end+1:]
                                
    #                             # 处理接收到的命令
    #                             self.process_received_command(cmd)
    #                             self.last_received_time = time.time()
    #                         else:
    #                             break
    #                 except Exception as e:
    #                     self.get_logger().error(f"读取或解析串口数据异常: {str(e)}")
    #                     buffer = ""  # 清空缓冲区，避免错误累积
                
    #             # 防止CPU过载
    #             time.sleep(0.01)
            
    #         except Exception as e:
    #             self.get_logger().error(f"通信线程异常: {str(e)}")
    #             # 尝试重新连接
    #             try:
    #                 self.reconnect()
    #             except Exception as reconnect_error:
    #                 self.get_logger().error(f"重连过程异常: {str(reconnect_error)}")
                    
    #             time.sleep(1.0)


#####################################################
    # def communication_loop(self):
    #     """通信线程主循环 - 健壮版"""
    #     self.get_logger().info("通信线程已启动")
        
    #     buffer = ""
    #     last_flush_time = time.time()
    #     flush_interval = 5.0  # 定期清理缓冲区，避免累积过多无效数据
    #     reconnect_timeout = 5.0  # 重连等待时间
    #     last_reconnect_attempt = 0
        
    #     while self.running:
    #         try:
    #             current_time = time.time()
                
    #             # 定期清理缓冲区，防止累积无效数据
    #             if current_time - last_flush_time > flush_interval:
    #                 if len(buffer) > 1024:  # 如果缓冲区太大却没有找到有效命令
    #                     self.get_logger().warn(f"清理过大的缓冲区: {len(buffer)} 字节")
    #                     buffer = buffer[-1024:]  # 保留后1KB，防止切断命令
    #                 last_flush_time = current_time
                
    #             # 限制命令队列大小，防止内存溢出
    #             while self.tx_queue.qsize() > 100:  # 最多保留100条命令
    #                 try:
    #                     self.tx_queue.get(block=False)
    #                     self.get_logger().warn("命令队列溢出，丢弃老命令")
    #                 except:
    #                     break
                
    #             # 检查串口状态，如果已断开则尝试重连
    #             if not self.is_connected or not hasattr(self, 'ser') or not self.ser or not self.ser.is_open:
    #                 if current_time - last_reconnect_attempt > reconnect_timeout:
    #                     self.get_logger().info("检测到串口断开，尝试重新连接")
    #                     try:
    #                         self.reconnect()
    #                     except Exception as e:
    #                         self.get_logger().error(f"重连失败: {e}")
    #                     last_reconnect_attempt = current_time
    #                 time.sleep(0.5)  # 减少CPU占用
    #                 continue
                
    #             # 检查发送队列
    #             if not self.tx_queue.empty() and self.is_connected:
    #                 try:
    #                     cmd = self.tx_queue.get(block=False)
    #                     self.ser.write(cmd.encode())
    #                     self.get_logger().debug(f"发送: {cmd.strip()}")
    #                 except Exception as e:
    #                     self.get_logger().error(f"发送命令失败: {e}")
    #                     self.is_connected = False
    #                     continue
                
    #             # 检查接收数据
    #             if self.is_connected and self.ser.in_waiting:
    #                 try:
    #                     data = self.ser.read(self.ser.in_waiting).decode('utf-8', errors='ignore')
    #                     buffer += data
                        
    #                     # 命令计数，避免无限循环处理
    #                     cmd_count = 0
    #                     max_cmds_per_cycle = 10
                        
    #                     # 处理完整的命令
    #                     while cmd_count < max_cmds_per_cycle:
    #                         start = buffer.find('<')
    #                         if start == -1:
    #                             # 没有找到开始标记，清空缓冲区
    #                             buffer = ""
    #                             break
                                
    #                         end = buffer.find('>', start)
    #                         if end == -1:
    #                             # 没有找到结束标记，保留数据等待更多输入
    #                             break
                            
    #                         # 提取命令
    #                         cmd = buffer[start+1:end]
    #                         buffer = buffer[end+1:]
                            
    #                         # 处理接收到的命令
    #                         try:
    #                             self.process_received_command(cmd)
    #                             self.last_received_time = time.time()
    #                         except Exception as cmd_e:
    #                             self.get_logger().error(f"处理命令 '{cmd}' 时出错: {cmd_e}")
                            
    #                         cmd_count += 1
                        
    #                     # 如果处理了最大命令数，记录警告
    #                     if cmd_count == max_cmds_per_cycle:
    #                         self.get_logger().warn(f"单次循环处理了大量命令 ({max_cmds_per_cycle})，可能有性能问题")
                            
    #                 except Exception as e:
    #                     self.get_logger().error(f"读取或解析串口数据异常: {e}")
    #                     # 不完全清空缓冲区，保留一部分避免丢失命令
    #                     if len(buffer) > 100:
    #                         buffer = buffer[-100:]
                
    #             # 防止CPU过载
    #             time.sleep(0.01)
            
    #         except Exception as e:
    #             self.get_logger().error(f"通信线程异常: {e}")
    #             # 尝试恢复
    #             try:
    #                 # 关闭串口
    #                 if hasattr(self, 'ser') and self.ser is not None and self.ser.is_open:
    #                     try:
    #                         self.ser.close()
    #                     except:
    #                         pass
                    
    #                 self.is_connected = False
    #                 time.sleep(1.0)  # 等待一段时间再重试
    #             except Exception as recover_e:
    #                 self.get_logger().error(f"恢复过程发生错误: {recover_e}")
    #                 time.sleep(2.0)  # 错误恢复也失败，增加等待时间

    # def reconnect(self):
    #     """尝试重新连接串口 - 健壮版"""
    #     # 先断开现有连接
    #     if hasattr(self, 'ser') and self.ser is not None:
    #         try:
    #             if self.ser.is_open:
    #                 self.ser.close()
    #         except Exception as e:
    #             self.get_logger().warn(f"关闭现有串口时出错: {e}")
        
    #     self.is_connected = False
    #     self.get_logger().info("尝试重新连接串口...")
        
    #     # 如果自动发现模式，重新搜索设备
    #     if self.auto_discover:
    #         new_port = self.discover_serial_port()
    #         if new_port and new_port != self.serial_port:
    #             self.get_logger().info(f"发现新端口: {new_port}，之前使用: {self.serial_port}")
    #             self.serial_port = new_port
        
    #     # 尝试建立新连接
    #     try:
    #         self.ser = serial.Serial(
    #             port=self.serial_port,
    #             baudrate=self.baud_rate,
    #             timeout=1.0
    #         )
            
    #         # 清空缓冲区
    #         self.ser.reset_input_buffer()
    #         self.ser.reset_output_buffer()
            
    #         self.is_connected = True
    #         self.get_logger().info(f"串口重新连接成功: {self.serial_port} @ {self.baud_rate}")
            
    #         # 发送初始化命令
    #         self.send_command("INIT")
            
    #         return True
    #     except Exception as e:
    #         self.get_logger().error(f"串口重新连接失败: {e}")
    #         self.is_connected = False
    #         return False
##################################################################
    def process_received_command(self, cmd):
        """处理从STM32接收到的命令 - 健壮版"""
        self.get_logger().debug(f"接收: {cmd}")
        
        try:
            # 验证命令格式
            if not cmd or ':' not in cmd:
                self.get_logger().warn(f"无效命令格式: {cmd}")
                return
            
            # 解析命令格式: TYPE:VALUE
            parts = cmd.split(':')
            cmd_type = parts[0]
            value = ':'.join(parts[1:])
            
            # 验证命令类型
            valid_types = ['STATUS', 'BATT', 'OK', 'ERROR', 'INFO', 'WARNING']
            if cmd_type not in valid_types:
                self.get_logger().warn(f"未知命令类型: {cmd_type}")
            
            # 处理不同类型的命令
            if cmd_type == 'STATUS':
                # 发布状态
                status_msg = String()
                status_msg.data = value
                self.status_pub.publish(status_msg)
            
            elif cmd_type == 'BATT':
                # 处理电池电压
                try:
                    voltage = float(value)
                    if 0.0 <= voltage <= 15.0:  # 合理的电压范围检查
                        battery_msg = Float32()
                        battery_msg.data = voltage
                        self.battery_pub.publish(battery_msg)
                    else:
                        self.get_logger().warn(f"电池电压值超出范围: {voltage}V")
                except ValueError:
                    self.get_logger().warn(f"无效的电池电压值: {value}")
            
            elif cmd_type == 'OK':
                # 命令确认
                self.get_logger().debug(f"命令执行成功: {value}")
            
            elif cmd_type == 'ERROR':
                # 错误报告
                self.get_logger().error(f"STM32 错误: {value}")
                # 可以在这里添加错误处理逻辑
            
            elif cmd_type == 'INFO' or cmd_type == 'WARNING':
                # 信息和警告
                log_method = self.get_logger().info if cmd_type == 'INFO' else self.get_logger().warn
                log_method(f"STM32 {cmd_type.lower()}: {value}")
            
            else:
                # 其他未知命令
                self.get_logger().info(f"收到未知命令: {cmd_type}:{value}")
        
        except Exception as e:
            self.get_logger().error(f"处理命令时出错: {str(e)}")

    def communication_loop(self):
        """通信线程主循环 - 线程安全版"""
        self.get_logger().info("通信线程已启动")
        
        buffer = ""
        last_flush_time = time.time()
        flush_interval = 5.0
        reconnect_timeout = 5.0
        last_reconnect_attempt = 0
        
        while self.running:
            try:
                current_time = time.time()
                
                # 定期清理缓冲区，防止累积无效数据
                if current_time - last_flush_time > flush_interval:
                    if len(buffer) > 1024:
                        self.get_logger().warn(f"清理过大的缓冲区: {len(buffer)} 字节")
                        buffer = buffer[-1024:]
                    last_flush_time = current_time
                
                # 限制命令队列大小
                while self.tx_queue.qsize() > 100:
                    try:
                        self.tx_queue.get(block=False)
                        self.get_logger().warn("命令队列溢出，丢弃老命令")
                    except:
                        break
                
                # 检查串口状态，如果已断开则尝试重连
                is_connected = False
                with self.serial_lock:
                    is_connected = self.is_connected and hasattr(self, 'ser') and self.ser and self.ser.is_open
                
                if not is_connected:
                    if current_time - last_reconnect_attempt > reconnect_timeout:
                        self.get_logger().info("检测到串口断开，尝试重新连接")
                        try:
                            with self.serial_lock:
                                self.reconnect()
                        except Exception as e:
                            self.get_logger().error(f"重连失败: {e}")
                        last_reconnect_attempt = current_time
                    time.sleep(0.5)
                    continue
                
                # 检查发送队列
                if not self.tx_queue.empty():
                    with self.serial_lock:
                        is_connected = self.is_connected and self.ser and self.ser.is_open
                        if is_connected:
                            try:
                                cmd = self.tx_queue.get(block=False)
                                self.ser.write(cmd.encode())
                                self.get_logger().debug(f"发送: {cmd.strip()}")
                            except Exception as e:
                                self.get_logger().error(f"发送命令失败: {e}")
                                self.is_connected = False
                                continue
                
                # 检查接收数据
                with self.serial_lock:
                    is_connected = self.is_connected and self.ser and self.ser.is_open
                    if is_connected and self.ser.in_waiting:
                        try:
                            data = self.ser.read(self.ser.in_waiting).decode('utf-8', errors='ignore')
                            buffer += data
                        except Exception as e:
                            self.get_logger().error(f"读取串口数据异常: {e}")
                            self.is_connected = False
                            continue
                
                # 处理完整的命令
                cmd_count = 0
                max_cmds_per_cycle = 10
                
                while cmd_count < max_cmds_per_cycle:
                    start = buffer.find('<')
                    if start == -1:
                        buffer = ""
                        break
                        
                    end = buffer.find('>', start)
                    if end == -1:
                        break
                    
                    # 提取命令
                    cmd = buffer[start+1:end]
                    buffer = buffer[end+1:]
                    
                    # 处理接收到的命令
                    try:
                        self.process_received_command(cmd)
                        self.last_received_time = time.time()
                    except Exception as cmd_e:
                        self.get_logger().error(f"处理命令 '{cmd}' 时出错: {cmd_e}")
                    
                    cmd_count += 1
                
                # 防止CPU过载
                time.sleep(0.01)
            
            except Exception as e:
                self.get_logger().error(f"通信线程异常: {e}")
                # 尝试恢复
                try:
                    with self.serial_lock:
                        # 关闭串口
                        if hasattr(self, 'ser') and self.ser is not None and self.ser.is_open:
                            try:
                                self.ser.close()
                            except:
                                pass
                        
                        self.is_connected = False
                    time.sleep(1.0)
                except Exception as recover_e:
                    self.get_logger().error(f"恢复过程发生错误: {recover_e}")
                    time.sleep(2.0)

    def reconnect(self):
        """尝试重新连接串口 - 线程安全版"""
        with self.serial_lock:
            # 先断开现有连接
            if hasattr(self, 'ser') and self.ser is not None:
                try:
                    if self.ser.is_open:
                        self.ser.close()
                except Exception as e:
                    self.get_logger().warn(f"关闭现有串口时出错: {e}")
            
            self.is_connected = False
            self.get_logger().info("尝试重新连接串口...")
            
            # 如果自动发现模式，重新搜索设备
            if self.auto_discover:
                new_port = self.discover_serial_port()
                if new_port and new_port != self.serial_port:
                    self.get_logger().info(f"发现新端口: {new_port}，之前使用: {self.serial_port}")
                    self.serial_port = new_port
            
            # 尝试建立新连接
            try:
                # 检查设备存在
                import os
                if not os.path.exists(self.serial_port):
                    self.get_logger().error(f"串口设备不存在: {self.serial_port}")
                    return False
                    
                self.ser = serial.Serial(
                    port=self.serial_port,
                    baudrate=self.baud_rate,
                    timeout=1.0
                )
                
                # 清空缓冲区
                self.ser.reset_input_buffer()
                self.ser.reset_output_buffer()
                
                self.is_connected = True
                self.get_logger().info(f"串口重新连接成功: {self.serial_port} @ {self.baud_rate}")
                
                # 发送初始化命令
                self.send_command("INIT")
                
                return True
            except Exception as e:
                self.get_logger().error(f"串口重新连接失败: {e}")
                self.is_connected = False
                return False


    # def process_received_command(self, cmd):
    #     """处理从STM32接收到的命令"""
    #     self.get_logger().debug(f"接收: {cmd}")
        
    #     try:
    #         # 解析命令格式: TYPE:VALUE
    #         parts = cmd.split(':')
    #         if len(parts) < 2:
    #             return
            
    #         cmd_type = parts[0]
    #         value = ':'.join(parts[1:])
            
    #         if cmd_type == 'STATUS':
    #             # 发布状态
    #             status_msg = String()
    #             status_msg.data = value
    #             self.status_pub.publish(status_msg)
            
    #         elif cmd_type == 'BATT':
    #             # 处理电池电压
    #             try:
    #                 voltage = float(value)
    #                 battery_msg = Float32()
    #                 battery_msg.data = voltage
    #                 self.battery_pub.publish(battery_msg)
    #             except ValueError:
    #                 pass
            
    #         elif cmd_type == 'OK':
    #             # 命令确认
    #             pass
            
    #         elif cmd_type == 'ERROR':
    #             # 错误报告
    #             self.get_logger().error(f"STM32 错误: {value}")
            
    #         else:
    #             # 其他未知命令
    #             self.get_logger().info(f"收到未知命令: {cmd_type}:{value}")
        
    #     except Exception as e:
    #         self.get_logger().error(f"处理命令时出错: {str(e)}")
    
    # def watchdog_callback(self):
    #     """看门狗定时器回调，确保机器人在没有命令时停止"""
    #     current_time = time.time()
        
    #     # 检查命令超时
    #     if current_time - self.last_cmd_time > 5:
    #         # 发送停止命令
    #        self.send_motor_command(0.0, 0.0)
    #         # pass
    #     # 检查通信状态
    #     if current_time - self.last_received_time > 5.0 and self.is_connected:
    #         self.get_logger().warn("通信超时，尝试重新连接")
    #         self.reconnect()
    def watchdog_callback(self):
        """看门狗定时器回调，确保机器人在没有命令时停止 - 线程安全版"""
        current_time = time.time()
        
        # 检查命令超时
        if current_time - self.last_cmd_time > self.cmd_timeout:
            # 发送停止命令 - 使用send_command而非直接访问串口
            self.send_motor_command(0.0, 0.0)
    # def reconnect(self):
    #     """尝试重新连接串口"""
    #     try:
    #         if self.ser is not None and self.ser.is_open:
    #             self.ser.close()
            
    #         self.is_connected = False
    #         self.get_logger().info("尝试重新连接串口...")
            
    #         # 重新初始化串口
    #         self.init_serial()
            
    #     except Exception as e:
    #         self.get_logger().error(f"重新连接失败: {str(e)}")
    
    def publish_diagnostics(self):
        """发布诊断信息"""
        diag_array = DiagnosticArray()
        diag_array.header.stamp = self.get_clock().now().to_msg()
        
        # 硬件状态
        hw_status = DiagnosticStatus()
        hw_status.name = "STM32 Controller"
        hw_status.hardware_id = self.serial_port
        
        if self.is_connected:
            time_since_last = time.time() - self.last_received_time
            if time_since_last < 1.0:
                hw_status.level = DiagnosticStatus.OK
                hw_status.message = "通信正常"
            elif time_since_last < 5.0:
                hw_status.level = DiagnosticStatus.WARN
                hw_status.message = "通信延迟"
            else:
                hw_status.level = DiagnosticStatus.ERROR
                hw_status.message = "通信丢失"
        else:
            hw_status.level = DiagnosticStatus.ERROR
            hw_status.message = "未连接"
        
        # 添加详细信息
        hw_status.values.append(KeyValue(key="端口", value=self.serial_port))
        hw_status.values.append(KeyValue(key="波特率", value=str(self.baud_rate)))
        hw_status.values.append(KeyValue(key="连接状态", value="已连接" if self.is_connected else "未连接"))
        hw_status.values.append(KeyValue(key="夹爪状态", value="已抓取" if self.gripper_state else "已释放"))
        
        # 添加到诊断数组
        diag_array.status.append(hw_status)
        
        # 发布诊断信息
        self.diagnostic_pub.publish(diag_array)
    
    def destroy_node(self):
        """节点销毁时清理资源"""
        self.get_logger().info("正在关闭硬件接口节点...")
        
        # 停止通信线程
        self.stop_communication_thread()
        
        # 停止所有电机
        if self.is_connected:
            try:
                self.send_motor_command(0.0, 0.0)
                time.sleep(0.1)  # 给命令一点时间发送
            except Exception as e:
                self.get_logger().error(f"发送停止命令时出错: {str(e)}")
        
        # 关闭串口
        if hasattr(self, 'ser') and self.ser is not None and self.ser.is_open:
            self.ser.close()
            self.get_logger().info("串口已关闭")
        
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    
    node = HardwareInterface()
    
    # 创建多线程执行器
    executor = MultiThreadedExecutor(num_threads=4)
    executor.add_node(node)
    
    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()